/**
 * @FilePath: shared_parameters\src\param_server.cpp
 * @Description:  
 * @Author: haibo.yang
 * @Version: 1.0.0
 * @LastEditors: haibo haibo.yang@lgmgim.cn
 * @LastEditTime: 2025-02-19 14:37:47
 * @Copyright (c) 2025 by 临工智能信息科技有限公司, All Rights Reserved. 
**/

#include <rclcpp/rclcpp.hpp>
#include "shared_parameters/shared_params_interface.hpp"
#include "agv_srvs/srv/dyn_param.hpp"
#include "agv_srvs/srv/static_param.hpp"
#include <tinyxml2.h>
#include <filesystem>
#include <map>
#include <string>
#include <cstring> 
#include <jsoncpp/json/json.h>
#include <sstream>

using namespace tinyxml2;
namespace fs = std::filesystem;

constexpr const char* AGV_FILE_NAME = "/params/AGVParam.xml";
constexpr const char* CALIB_FILE_NAME = "/params/CalibParam.xml";
constexpr const char* ACTION_READ = "read";
constexpr const char* ACTION_WRITE = "write";
constexpr const char* ACTION_ROBOT_ID = "Robot_ID";
constexpr const char* ACTION_VEHICLE_ID = "Vehicle_ID";
constexpr const char* ROBOT_ID_KEY = "ROBOT_ID";

struct ErrorInfo {
    bool has_error;      // 错误标志
    int error_code;      // 错误码
    std::string message; // 错误描述
    ErrorInfo() : has_error(false), error_code(0), message("") {}
};

class ParamServer : public rclcpp::Node {
public:
    using DynParam = agv_srvs::srv::DynParam;
    using StaticParam = agv_srvs::srv::StaticParam;
    ParamServer() : Node("param_server") {
        SharedMemoryManager::cleanup();
        if (!SharedParamsInterface::initialize()) {
            SetError(1, "Failed to initialize shared memory interface");
            return;
        }
        LoadParams();

        dyn_param_service_ = this->create_service<DynParam>(
            "DynParam",
            std::bind(&ParamServer::DynParamHandle, this, std::placeholders::_1, std::placeholders::_2)
        );
        static_param_service_ = this->create_service<StaticParam>(
            "StaticParam",
            std::bind(&ParamServer::StaticParamHandle, this, std::placeholders::_1, std::placeholders::_2)
        );
    }
private:
    struct XML_DATA_TYPE {
        const std::string int_key = "int";
        const std::string double_key = "double";
        const std::string float_key = "float";
        const std::string bool_key = "bool";
        const std::string string_key = "string";
    };

    void SetError(int code, const std::string& message, bool ignore_errors = false) {
        if (!ignore_errors) {
            error_info_.has_error = true;
            error_info_.error_code = code;
            error_info_.message = message;

            WriteStatus();
        }
        RCLCPP_ERROR(this->get_logger(), "Error: [%d] %s", code, message.c_str());
    }

    void WriteStatus() {
        // write params state
        SharedParamsInterface::setParam("shared_parameters", "error_code", error_info_.error_code);   
        SharedParamsInterface::setParam("shared_parameters", "error_message", error_info_.message);    
    }

    SharedParamsInterface::ParamValue ConvertToParamValue(const std::string& value, const std::string& dataType) {
        try {
            if (dataType == "int" || dataType == "INT") {
                return SharedParamsInterface::ParamValue(std::stoi(value));
            } else if (dataType == "float" || dataType == "FLOAT" || dataType == "double" || dataType == "DOUBLE") {
                return SharedParamsInterface::ParamValue(std::stod(value));
            } else if (dataType == "bool" || dataType == "BOOL") {
                return SharedParamsInterface::ParamValue(value == "true" || value == "1");
            } else if (dataType == "str" || dataType == "STRING") {
                return SharedParamsInterface::ParamValue(value);
            } else {
                return SharedParamsInterface::ParamValue(value);
            }
        } catch (const std::exception& e) {
            SetError(2, "Error converting value '" + value + "' of type '" + dataType + "': " + e.what());
            throw;
        }
    }

    // 处理单个Component节点
    void ProcessComponent(XMLElement* component, const std::string& parentPath = "") {
        if (!component) return;

        const char* symbolName = component->Attribute("SymbolName");
        if (!symbolName) return;

        if (parentPath.compare("GeneralParam") == 0) {
            node_names_.push_back(symbolName);
            return;
        }

        const char* dataType = component->Attribute("DataType");
        const char* value = component->Attribute("Value");
     
        if (dataType && value && strlen(dataType) && strlen(value)) {
            try {
                SharedParamsInterface::ParamValue paramValue = ConvertToParamValue(value, dataType);
                SharedParamsInterface::setParam(parentPath, symbolName, paramValue.value);
                RCLCPP_INFO(this->get_logger(), "Stored parameter: %s/%s = %s (%s)",
                            parentPath.c_str(), symbolName, value, dataType);
            } catch (const std::exception& e) {
                SetError(3, "Failed to store parameter " + std::string(symbolName) + ": " + e.what());
            }
        }
        std::string currentPath = parentPath.empty() ? symbolName : parentPath + "/" + symbolName;
        // 递归处理子节点
        for (XMLElement* child = component->FirstChildElement();
             child != nullptr;
             child = child->NextSiblingElement()) {
            if (strcmp(child->Name(), "Component") == 0) {
                ProcessComponent(child, currentPath);
            }
        }
    }

    // 处理treeComponent节点
    void ProcessTreeComponent(XMLElement* treeComponent, const std::string& parentPath) {
        if (!treeComponent) return;
        const char* symbolName = treeComponent->Attribute("SymbolName");
        if (!symbolName) return;
        std::string currentPath = parentPath.empty() ? symbolName : parentPath + "/" + symbolName;
        // 递归处理所有子Component节点
        for (XMLElement* child = treeComponent->FirstChildElement("Component");
             child != nullptr;
             child = child->NextSiblingElement("Component")) {
            ProcessComponent(child, currentPath);
        }
    }

    // 判断版本是否兼容
    bool IsVersionCompatible (XMLElement* element) {
        XMLElement* agvParam = nullptr;
        XMLElement* component = element->FirstChildElement("Component");
        while (component) {
            const char* symbolName = component->Attribute("SymbolName");
            if (symbolName && strcmp(symbolName, "AGVParam") == 0) {
                agvParam = component;
                break;
            }
            component = component->NextSiblingElement("Component");
        }

        if (!agvParam) {
            SetError(8, "AGVParam component not found");
            return false;
        }

        XMLElement* versionElement = nullptr;
        XMLElement* child = agvParam->FirstChildElement("Component");
        while (child) {
            const char* childSymbol = child->Attribute("SymbolName");
            if (childSymbol && strcmp(childSymbol, "version") == 0) {
                versionElement = child;
                break;
            }
            child = child->NextSiblingElement("Component");
        }

        if (!versionElement) {
            SetError(9, "Version element not found");
            return false;
        }

        // 获取版本值
        const char* versionValueStr = versionElement->Attribute("Value");
        if (!versionValueStr) {
            SetError(10, "Version Value attribute missing");
            return false;
        }

        float versionValue = atof(versionValueStr);
        if (versionValue < 2.0f) {
            SetError(11, "Version " + std::string(versionValueStr) + " is incompatible with ROS2");
            return false;
        }   

        return true;
    }

    XMLElement* LoadFile(const std::string filename, bool ignore_errors) {
        doc_.Clear();

        // 检查文件是否存在
        if (!fs::exists(filename)) {
            SetError(4, "Config file not found: " + filename, ignore_errors);
            return nullptr;
        }

        // 加载XML文件
        XMLError result = doc_.LoadFile(filename.c_str());
        if (result != XML_SUCCESS) {
            SetError(5, "Failed to load XML file: " + filename + " (Error: " + std::to_string(result) + ")", ignore_errors);
            return nullptr;
        }

        XMLElement* root = doc_.RootElement();
        if (!root) {
            SetError(6, "No root element found in XML");
            return nullptr;
        }

        // 处理AgvType节点
        XMLElement* agvType = root->FirstChildElement("AgvType");
        if (!agvType) {
            SetError(7, "No AgvType element found", ignore_errors);
            return nullptr;
        }

        if (!ignore_errors && !IsVersionCompatible(agvType)) {
            return nullptr;
        }

        return agvType;
    }

    void LoadParams() {
        // 获取包的安装路径
        std::string pkgPath = std::string(getenv("HOME"));
        std::string configfile1 = pkgPath + "/params/AGVParam.xml";
        std::string configfile2 = pkgPath + "/params/CalibParam.xml";
        std::vector<std::string> configfiles = {configfile1, configfile2};
        for (auto& file:configfiles) {
            bool can_ignore_file = file.find("AGVParam.xml") == std::string::npos;
            RCLCPP_INFO(this->get_logger(), "Loading XML from: %s", file.c_str());
            XMLElement* agvType = LoadFile(file, can_ignore_file);
            
            if (agvType) {
                // 处理所有顶层节点
                for (XMLElement* child = agvType->FirstChildElement();
                    child != nullptr;
                    child = child->NextSiblingElement()) {
                    RCLCPP_INFO(this->get_logger(), "Top component:%s", child->Name());
                    if (strcmp(child->Name(), "treeComponent") == 0) {
                        ProcessTreeComponent(child, "");
                    } else if (strcmp(child->Name(), "Component") == 0) {
                        ProcessComponent(child, "");
                    }
                }
            } else if (!can_ignore_file) {
                RCLCPP_ERROR(this->get_logger(), "load paramerters failed!");
                return;
            }
        }

        // 验证参数加载
        try {
            auto allParams = SharedParamsInterface::listAllParams();
            RCLCPP_INFO(this->get_logger(), "\n\n##########################\nTotal parameters loaded: %zu", allParams.size());
            for (const auto& param : allParams) {
                RCLCPP_INFO(this->get_logger(), "Loaded: %s/%s = %s (%s)",
                            param.node_name.c_str(),
                            param.param_name.c_str(),
                            param.param_value.c_str(),
                            param.param_type.c_str());
            }
        } catch (const std::exception& e) {
            SetError(12, "Error verifying parameters: " + std::string(e.what()));
        }

        WriteStatus();
    }

    std::string CheckNode(const std::string name) {
        auto node_names = this->get_node_names();
        RCLCPP_INFO(this->get_logger(), "Found %lu active nodes", node_names.size());
        
        std::string target_node_name = name; 
        auto it = std::find_if(node_names.begin(), node_names.end(), 
            [&](const std::string& full_name) {
                std::string normalized_name = full_name;
                if (!normalized_name.empty() && normalized_name[0] == '/') {
                    normalized_name.erase(0, 1);
                }
                return (target_node_name.find(normalized_name) != std::string::npos);
            });
        
        if (it == node_names.end()) {
            RCLCPP_WARN(this->get_logger(), "Node '%s' not found", target_node_name.c_str());
            return "";
        }
        
        std::string full_node_name = *it;
        RCLCPP_INFO(this->get_logger(), "Found node: %s", full_node_name.c_str());
        return full_node_name;
    }
    
    void CheckParameterExists(const std::string& node_name, const std::string& param_key) {
        auto client = this->create_client<rcl_interfaces::srv::ListParameters>(
            node_name + "/list_parameters");
        
        if (!client->wait_for_service(std::chrono::seconds(3))) {
            RCLCPP_ERROR(this->get_logger(), "Service not available for node: %s", node_name.c_str());
            return;
        }
        
        auto request = std::make_shared<rcl_interfaces::srv::ListParameters::Request>();
        request->prefixes = {param_key};
        request->depth = rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE;
        
        auto future = client->async_send_request(request);
        
        if (future.wait_for(std::chrono::seconds(3)) != std::future_status::ready) {
            RCLCPP_ERROR(this->get_logger(), "Service call timeout for node: %s", node_name.c_str());
            return;
        }
        
        auto response = future.get();
        bool param_exists = false;
        
        for (const auto& name : response->result.names) {
            if (name == param_key) {
                param_exists = true;
                break;
            }
        }
        
        if (param_exists) {
            RCLCPP_INFO(this->get_logger(), "Node '%s' has parameter '%s'", 
                       node_name.c_str(), param_key.c_str());
            GetParameterValue(node_name, param_key);
        } else {
            RCLCPP_WARN(this->get_logger(), "Node '%s' does NOT have parameter '%s'", 
                       node_name.c_str(), param_key.c_str());
        }
    }
    
    void GetParameterValue(const std::string& node_name, const std::string& param_key) {
        auto client = this->create_client<rcl_interfaces::srv::GetParameters>(
            node_name + "/get_parameters");
        
        if (!client->wait_for_service(std::chrono::seconds(3))) {
            RCLCPP_ERROR(this->get_logger(), "GetParameters service not available for node: %s", 
                       node_name.c_str());
            return;
        }
        
        auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
        request->names = {param_key};
        
        auto future = client->async_send_request(request);
        
        if (future.wait_for(std::chrono::seconds(3)) != std::future_status::ready) {
            RCLCPP_ERROR(this->get_logger(), "GetParameters service call timeout for node: %s", 
                       node_name.c_str());
            return;
        }
        
        auto response = future.get();
        if (!response->values.empty()) {
            const auto& param_value = response->values[0];
            if (param_value.type == rcl_interfaces::msg::ParameterType::PARAMETER_STRING) {
                RCLCPP_INFO(this->get_logger(), "Parameter value: %s", 
                           param_value.string_value.c_str());
            } else if (param_value.type == rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) {
                RCLCPP_INFO(this->get_logger(), "Parameter value: %ld", 
                           param_value.integer_value);
            } else if (param_value.type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
                RCLCPP_INFO(this->get_logger(), "Parameter value: %f", 
                           param_value.double_value);
            } else if (param_value.type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
                RCLCPP_INFO(this->get_logger(), "Parameter value: %s", 
                           param_value.bool_value ? "true" : "false");
            } else {
                RCLCPP_INFO(this->get_logger(), "Parameter type: %d", param_value.type);
            }
        }
    }

    std::pair<std::string, std::string> ParseString(const std::string& input) {
        size_t pos = input.find('/');
        if (pos != std::string::npos) {
            return {input.substr(0, pos), input.substr(pos + 1)};
        } else {
            return {input, ""};
        }
    }

    void DynParamHandle(const std::shared_ptr<DynParam::Request> request,
                    std::shared_ptr<DynParam::Response> response) {
        std::string node_name = request->node_name;
        std::string param_key = request->param_key;
        std::string param_value = request->param_value;

        if (node_name.empty() || param_key.empty() || param_value.empty()) {
            response->ack_message = "Error: Missing parameters in request";
            RCLCPP_ERROR(get_logger(), response->ack_message.c_str());
            return;
        }
        
        // 首先检查目标节点是否存在
        auto node_names = this->get_node_graph_interface()->get_node_names();
        std::string target_node = CheckNode(node_name);
        bool node_exists = false;
        
        for (const auto& name : node_names) {
            if (name == target_node) {
                node_exists = true;
                break;
            }
        }
        
        if (!node_exists) {
            response->ack_message = "Node " + target_node + " does not exist";
            RCLCPP_ERROR(get_logger(), response->ack_message.c_str());
            
            RCLCPP_INFO(get_logger(), "Available nodes:");
            for (const auto& name : node_names) {
                RCLCPP_INFO(get_logger(), "  %s", name.c_str());
            }
            return;
        }
        
        auto shared_param = ParseString(node_name);
        if(SharedParamsInterface::setParam(shared_param.first, shared_param.second + "/" + param_key, param_value)) {
            RCLCPP_INFO_STREAM(get_logger(), 
                "Parameter set for node: " << shared_param.first << 
                ", key: " << shared_param.second + "/" + param_key << 
                ", value: " << param_value);
        } else {
            response->ack_message = "Set parameter error!";
            return;
        }
        
        auto param_client_node = std::make_shared<rclcpp::Node>("param_client_tmp_node");
        auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(param_client_node, target_node);
        
        int max_retries = 5;
        int retry_count = 0;
        bool service_available = false;
        
        while (retry_count < max_retries && !service_available) {
            if (parameters_client->wait_for_service(std::chrono::seconds(3))) {
                service_available = true;
                break;
            }
            RCLCPP_WARN(get_logger(), "Parameter service not available, retrying (%d/%d)...", 
                        retry_count + 1, max_retries);
            retry_count++;
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
        }

        if (!service_available) {
            response->ack_message = "Parameter service for node " + target_node + " not available after " + 
                                   std::to_string(max_retries) + " attempts";
            RCLCPP_ERROR(get_logger(), response->ack_message.c_str());
            return;
        }
        
        try {
            auto param_value_msg = DetectAndConvertParamValue(param_value);
            auto result = parameters_client->set_parameters({
                rclcpp::Parameter(param_key, param_value_msg)
            });
            
            if (result[0].successful) {
                response->ack_message = "Successfully set parameter '" + param_key + 
                                       "' on node '" + target_node + "' to: " + param_value;
                RCLCPP_INFO(get_logger(), response->ack_message.c_str());
            } else {
                response->ack_message = "Failed to set parameter: " + result[0].reason;
                RCLCPP_ERROR(get_logger(), response->ack_message.c_str());
            }
        } catch (const std::exception& e) {
            response->ack_message = "Exception when setting parameter: " + std::string(e.what());
            RCLCPP_ERROR(get_logger(), response->ack_message.c_str());
        }
    }

    rcl_interfaces::msg::ParameterValue DetectAndConvertParamValue(const std::string& str_value) {
        rcl_interfaces::msg::ParameterValue value;
        
        // 去除字符串前后的空白字符
        std::string trimmed_value = str_value;
        trimmed_value.erase(0, trimmed_value.find_first_not_of(" \t\n\r\f\v"));
        trimmed_value.erase(trimmed_value.find_last_not_of(" \t\n\r\f\v") + 1);
        
        if (trimmed_value == "true" || trimmed_value == "1" || 
            trimmed_value == "yes" || trimmed_value == "on" ||
            trimmed_value == "True" || trimmed_value == "TRUE") {
            value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
            value.bool_value = true;
            return value;
        }
        
        if (trimmed_value == "false" || trimmed_value == "0" || 
            trimmed_value == "no" || trimmed_value == "off" ||
            trimmed_value == "False" || trimmed_value == "FALSE") {
            value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
            value.bool_value = false;
            return value;
        }
        
        try {
            // 检查是否包含小数点或指数符号，如果有则不是整数
            if (trimmed_value.find('.') == std::string::npos && 
                trimmed_value.find('e') == std::string::npos && 
                trimmed_value.find('E') == std::string::npos) {
                size_t pos;
                int int_val = std::stoi(trimmed_value, &pos);
                // 确保整个字符串都被转换
                if (pos == trimmed_value.length()) {
                    value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
                    value.integer_value = int_val;
                    return value;
                }
            }
        } catch (...) {
            // 转换失败，继续尝试其他类型
        }
        
        try {
            size_t pos;
            double double_val = std::stod(trimmed_value, &pos);
            // 确保整个字符串都被转换
            if (pos == trimmed_value.length()) {
                value.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
                value.double_value = double_val;
                return value;
            }
        } catch (...) {
            // 转换失败，继续尝试其他类型
        }
        
        if (trimmed_value.front() == '[' && trimmed_value.back() == ']') {
            std::string array_content = trimmed_value.substr(1, trimmed_value.length() - 2);
            std::vector<std::string> elements;
            std::stringstream ss(array_content);
            std::string item;
            
            while (std::getline(ss, item, ',')) {
                // 去除每个元素前后的空白
                item.erase(0, item.find_first_not_of(" \t\n\r\f\v"));
                item.erase(item.find_last_not_of(" \t\n\r\f\v") + 1);
                elements.push_back(item);
            }
            
            try {
                std::vector<int64_t> int_array;
                bool all_integers = true;
                
                for (const auto& elem : elements) {
                    if (elem.find('.') != std::string::npos || 
                        elem.find('e') != std::string::npos || 
                        elem.find('E') != std::string::npos) {
                        all_integers = false;
                        break;
                    }
                    
                    size_t pos;
                    int64_t int_val = std::stoll(elem, &pos);
                    if (pos != elem.length()) {
                        all_integers = false;
                        break;
                    }
                    int_array.push_back(int_val);
                }
                
                if (all_integers && !int_array.empty()) {
                    value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY;
                    value.integer_array_value = int_array;
                    return value;
                }
            } catch (...) {
                // 转换失败，继续尝试其他类型
            }
            
            try {
                std::vector<double> double_array;
                bool all_doubles = true;
                
                for (const auto& elem : elements) {
                    size_t pos;
                    double double_val = std::stod(elem, &pos);
                    if (pos != elem.length()) {
                        all_doubles = false;
                        break;
                    }
                    double_array.push_back(double_val);
                }
                
                if (all_doubles && !double_array.empty()) {
                    value.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY;
                    value.double_array_value = double_array;
                    return value;
                }
            } catch (...) {
                // 转换失败，继续尝试其他类型
            }
            
            try {
                std::vector<bool> bool_array; // 使用 std::vector<bool> 而不是 std::vector<uint8_t>
                bool all_bools = true;
                
                for (const auto& elem : elements) {
                    std::string lower_elem = elem;
                    std::transform(lower_elem.begin(), lower_elem.end(), lower_elem.begin(), ::tolower);
                    
                    if (lower_elem == "true" || lower_elem == "1" || 
                        lower_elem == "yes" || lower_elem == "on") {
                        bool_array.push_back(true);
                    } else if (lower_elem == "false" || lower_elem == "0" || 
                              lower_elem == "no" || lower_elem == "off") {
                        bool_array.push_back(false);
                    } else {
                        all_bools = false;
                        break;
                    }
                }
                
                if (all_bools && !bool_array.empty()) {
                    value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY;
                    value.bool_array_value = bool_array; // 现在类型匹配了
                    return value;
                }
            } catch (...) {
                // 转换失败，继续尝试其他类型
            }
            
            try {
                std::vector<std::string> string_array;
                for (const auto& elem : elements) {
                    string_array.push_back(elem);
                }
                
                if (!string_array.empty()) {
                    value.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY;
                    value.string_array_value = string_array;
                    return value;
                }
            } catch (...) {
                // 转换失败，继续尝试其他类型
            }
        }
        
        if (trimmed_value.size() >= 2 && 
            trimmed_value.substr(0, 2) == "0x" &&
            trimmed_value.size() % 2 == 0) {
            try {
                std::vector<uint8_t> byte_array;
                for (size_t i = 2; i < trimmed_value.size(); i += 2) {
                    std::string byte_str = trimmed_value.substr(i, 2);
                    uint8_t byte_val = static_cast<uint8_t>(std::stoul(byte_str, nullptr, 16));
                    byte_array.push_back(byte_val);
                }
                
                value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY;
                value.byte_array_value = byte_array;
                return value;
            } catch (...) {
                // 转换失败，继续尝试其他类型
            }
        }
        
        // 默认为字符串类型
        value.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
        value.string_value = str_value;
        return value;
    }

    void StaticParamHandle(const std::shared_ptr<StaticParam::Request> request,
                std::shared_ptr<StaticParam::Response> response) {
        bool ret = false;

        std::string home_dir = (getenv("HOME") ? getenv("HOME") : "");
        std::string cur_file_path = home_dir + AGV_FILE_NAME;
        std::string cur_file_path_calib = home_dir + CALIB_FILE_NAME;

        RCLCPP_INFO(get_logger(), "Current param path: %s", cur_file_path.c_str());

        std::string action = request->action_name;
        if (action == ACTION_READ) {
            std::vector<std::string> params;
            ReadFromParams(params);
            response->ack_message = FomartJsonParams(params);
        } else if (action == ACTION_WRITE) {
            response->ack_message = "writeToParamServer:";
            LoadParams();
        } else if (action == ACTION_ROBOT_ID || action == ACTION_VEHICLE_ID) {
           int robot_id = -1;
           const char* ID = std::getenv("Robot_ID");
           if (ID != nullptr) {
                robot_id = std::stoi(ID);
            } else {
                RCLCPP_ERROR(rclcpp::get_logger("logger"), "未找到 Robot_ID 环境变量");
            }
          
            RCLCPP_INFO_STREAM(get_logger(), "robot_id=" << robot_id);

            response->ack_message = "writeToRobID";
            response->robot_id = robot_id;
            
            RCLCPP_INFO(get_logger(), "RobotID read: message=%s, ID=%d", 
                        response->ack_message.c_str(), robot_id);
        } else {
            RCLCPP_ERROR(get_logger(), "Unknown action: %s", action.c_str());
            response->ack_message = "unknown action static param";
        }
    }

    bool ReadFromParams(std::vector<std::string>& params) {
        try {
            auto allParams = SharedParamsInterface::listAllParams();
            params.clear();
            
            for (const auto& param : allParams) {
                std::string full_name = param.node_name + "/" + param.param_name;
                params.push_back(TypeSwitch(full_name, param.param_value, param.param_type));
                
                RCLCPP_DEBUG(get_logger(), "Read parameter: %s (%s) = %s", 
                            full_name.c_str(), 
                            param.param_type.c_str(),
                            param.param_value.c_str());
            }
            
            return true;
        } catch (const std::exception& e) {
            SetError(18, "Failed to read parameters: " + std::string(e.what()));
            return false;
        }
    }

    std::string TypeSwitch(const std::string& key, 
        const std::string& value, const std::string& type) {
        Json::Value root;
        XML_DATA_TYPE data_type;
        root[key] = value;
        if (type == "INT") {
            root["type"] = data_type.int_key;
        } else if (type == "DOUBLE") {
            root["type"] = data_type.double_key;
        } else if (type == "BOOL") {
            root["type"] = data_type.bool_key;
        } else if (type == "STRING") {
            root["type"] = data_type.string_key;
        } else {
            RCLCPP_WARN(get_logger(), "Unsupported type for key: %s", key.c_str());
            return "";
        }
        
        // RCLCPP_INFO_STREAM(get_logger(), "root[" << key << "]=" << root[key] << "(" <<  root["type"] << ")");
        Json::FastWriter writer;
        return writer.write(root);
    }

    bool ParseStringJson(const std::string& json_str, 
                         std::string& data_type, 
                         std::string& data_name, 
                         Json::Value& value_obj) {
        Json::CharReaderBuilder builder;
        Json::Value root;
        std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
        std::string errors;
        
        const char* begin = json_str.c_str();
        const char* end = begin + json_str.size();
        
        if (!reader->parse(begin, end, &root, &errors)) {
            RCLCPP_ERROR(get_logger(), "Failed to parse JSON: %s", errors.c_str());
            return false;
        }
        
        if (root.isMember("type")) {
            data_type = root["type"].asString();
        }
        
        for (auto it = root.begin(); it != root.end(); ++it) {
            std::string key = it.name();
            if (key != "type") {
                data_name = key;
                value_obj = *it;
                return true;
            }
        }
        
        RCLCPP_ERROR(get_logger(), "No valid data field found in JSON");
        return false;
    }

    std::string FomartJsonParams(const std::vector<std::string>& params) {
        Json::Value root(Json::objectValue);

        for (const auto& param : params) {
            std::string data_type, data_name;
            Json::Value value_obj;
            
            if (!ParseStringJson(param, data_type, data_name, value_obj)) {
                RCLCPP_ERROR(get_logger(), "Failed to parse parameter JSON: %s", param.c_str());
                continue;
            }

            if (!data_name.empty() && data_name[0] == '/') {
                data_name = data_name.substr(1);
            }

            std::vector<std::string> path_parts;
            std::istringstream path_stream(data_name);
            std::string part;
            while (std::getline(path_stream, part, '/')) {
                path_parts.push_back(part);
            }

            Json::Value* current = &root;
            for (size_t i = 0; i < path_parts.size(); ++i) {
                const std::string& node_name = path_parts[i];
                
                if (i == path_parts.size() - 1) {
                    Json::Value param_node;
                    param_node["value"] = value_obj;
                    param_node["type"] = data_type;
                    (*current)[node_name] = param_node;
                } else {
                    if (!current->isMember(node_name)) {
                        (*current)[node_name] = Json::objectValue;
                    }
                    current = &(*current)[node_name];
                }
            }
        }

        Json::StreamWriterBuilder builder;
        builder["indentation"] = ""; // 紧凑格式
        return Json::writeString(builder, root);
    }

    // Todo: Modify and Update Parameters
    std::vector<std::string> node_names_;
    ErrorInfo error_info_;
    XMLDocument doc_;
    rclcpp::Service<DynParam>::SharedPtr dyn_param_service_;
    rclcpp::Service<StaticParam>::SharedPtr static_param_service_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ParamServer>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}